//============================================================================
// Name        : EasyParticleFilter.cpp
// Author      : 
// Version     :
// Copyright   : Your copyright notice
// Description : Hello World in C++, Ansi-style
//============================================================================

#include <iostream>
using namespace std;
#include "Maze.h"
#include "Robot.h"
#include "Particle.h"
#include "WeightedDistribution.h"
#include <vector>

#define ROBOT_HAS_COMPASS true
#define PARTICLE_COUNT 2000
#define PIC_WIDTH 500
#define PIC_HEIGHT 500


int main() {
	int maze_data [10][10] = {
				  { 1, 1, 0, 0, 2, 0, 0, 0, 0, 1 },
	              { 1, 2, 0, 0, 1, 1, 0, 0, 0, 0 },
	              { 0, 1, 1, 0, 0, 0, 0, 1, 0, 1 },
	              { 0, 0, 0, 0, 1, 0, 0, 1, 1, 2 },
	              { 1, 1, 0, 1, 1, 2, 0, 0, 1, 0 },
	              { 1, 1, 1, 0, 1, 1, 1, 0, 2, 0 },
	              { 2, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
	              { 1, 2, 0, 1, 1, 1, 1, 0, 0, 0 },
	              { 0, 0, 0, 0, 1, 0, 0, 0, 1, 0 },
	              { 0, 0, 1, 0, 0, 2, 1, 1, 1, 0 }};

	int * mazian = maze_data[0];
	int x = 10, y = 10;
	int mapx = PIC_WIDTH, mapy = PIC_HEIGHT;
	unsigned int numberOfParticles = PARTICLE_COUNT;
	bool showLikelyhoodMap = true;
	bool showMap = true;
	bool showMaze = true;
	bool showFilterParticles = true;
	bool hasFilter = true;

	double old_heading = 0;
	cout << "Making a Maze" <<endl;
	Maze world(mazian, x,y,mapx,mapy);
	cout << "Got a Maze" <<endl;
	Robot robbie(&world,x,y,mapx,mapy);
	robbie.setMaps(showMap, showLikelyhoodMap,showFilterParticles);
	if(hasFilter){
		robbie.initFilter(numberOfParticles);
	}
	robbie.enableFilter(hasFilter);
	cout << "Got a Robot" <<endl;
	srand(time(NULL));
	vector<Particle<int> > particles;
	vector<Particle<int> > * parts = &particles;
	Particle<int>::create_random(parts,numberOfParticles,&world);
	cout << "Got a Particles" <<endl;
	vector<Particle<int> > new_particles;
	new_particles.reserve(numberOfParticles);
	parts = &new_particles;
	cout << "drawing maze"<<endl;
	if(showMaze) world.draw();
	cout << "erstelle roboter particle" << endl;
	while(true){
		double r_d = robbie.read_sensor();
		for(unsigned int i = 0; i < numberOfParticles;i++){
			if(world.is_free(particles[i].p)){
				double p_d = particles[i].read_sensor(&world);
				particles[i].w = Particle<int>::w_gauss(r_d,p_d);
			}else{
				particles[i].w = 0;
			}
		}
		// displays ...
		if(showMaze) {
			world.redraw();
			world.drawRobot(robbie.getPosition());
			for(unsigned int i = 0; i < particles.size(); i++){
				world.drawParticles(particles[i].p);
			}
			world.repaint();
		}
		robbie.drawMap();
		//normalize
		double nu = 0;
		for( unsigned int i = 0; i < particles.size(); i++){
			nu+= particles[i].w;
		}
		for (unsigned int i = 0; i < particles.size(); i++) {
			particles[i].w /= nu;
		}
		//weighted distri
		WeightedDistribution<int> dist(particles);
		//pick
		Particle<int> p;
		for( unsigned int i = 0; i < numberOfParticles; i++){
			p = dist.pick();
			if( p.x == -1 && p.y == -1){
				Particle<int>::create_random(parts,1,&world);
			}else{
				if(dist.states.size() == 0){
					cout << "how comes I am here?" << endl;
				}
				new_particles.push_back(Particle<int>(p.x,p.y,(ROBOT_HAS_COMPASS?robbie.getHeading():p.heading),true));
			}
		}
		particles.clear();
		particles.insert(particles.begin(),new_particles.begin(), new_particles.end());
		new_particles.clear();
		//move things
		old_heading = robbie.getHeading();
		robbie.move();
		double d_h = robbie.getHeading() - old_heading;
		if(hasFilter)
			robbie.predictPosition(old_heading);
		//move particles
		for(unsigned int i = 0; i < particles.size(); i++){
			particles[i].heading += d_h;
			particles[i].advance_by(robbie.speed,&world);
		}
	}
	world.releaseImage();
	return 0;
}
